/***********************************************************************************************************
 * state_from_mavros.h
 * 
 *  Author: Wr
 *  Update Time: 2021.3.23
 *  Fuction:  连接mavros与px4_control的类             类的所有函数已经实现，也可在这里只声明，单独定义一个cpp文件实现
 * 读取： mavros的无人机状态量，用于px4控制   包括无人机状态、位置、速度、角度、角速度
 * 发布： 
 ***********************************************************************************************************/
 #ifndef STATE_FROM_MAVROS_H
 #define STATE_FROM_MAVROS_H

#include <ros/ros.h>

#include <Eigen/Eigen>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>

#include <sensor_msgs/Imu.h>

#include  <px4_msgs/Px4State.h>
#include "math_utils.h"


using namespace std;

class state_from_mavros
{
    public:
    state_from_mavros(void): state_nh("~")   
    { 
        //订阅 无人机状态
        state_sub = state_nh.subscribe<mavros_msgs::State>("/mavros/state", 10, &state_from_mavros::state_cb, this);
        //订阅 无人机当前位置  坐标系：ENU  (mavros将飞控在NED坐标系下的状态量转换到了ENU坐标系下)
        position_sub = state_nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, &state_from_mavros::position_cb, this);
        //订阅 无人机当前速度 坐标系：ENU 
        velocity_sub = state_nh.subscribe<geometry_msgs::TwistStamped>("/mavros/local_position/velocity_local", 10, &state_from_mavros::velocity_cb, this);
        //订阅 无人机姿态  坐标系：ENU
        attitude_sub = state_nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data", 10, &state_from_mavros::attitude_cb, this);
    
    } 
     //定义一个公有成员---无人机状态量，通过回调函数为其赋值，方便其他程序使用该变量
    px4_msgs::Px4State px4_state; 

    private:

        ros::NodeHandle state_nh;             //句柄
        ros::Subscriber state_sub ;             //     /mavros/state 的订阅者
        ros::Subscriber position_sub;       //
        ros::Subscriber velocity_sub;       //
        ros::Subscriber attitude_sub;      //   
        
        /* 回调函数   也可只在这里声明单独一个cpp文件实现
          * Fuction：获取无人机状态、位置、速度、姿态，赋值给自定义的消息
          * 
         */
        void state_cb(const mavros_msgs::State::ConstPtr& msg)
        { 
            px4_state.connected = msg->connected;
            px4_state.armed = msg->armed;
            px4_state.mode = msg->mode;
        }

        void position_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
        {
            px4_state.position[0] = msg->pose.position.x;
            px4_state.position[1] = msg->pose.position.y;
            px4_state.position[2] = msg->pose.position.z;
        }

        void velocity_cb(const geometry_msgs::TwistStamped::ConstPtr &msg)
        {
            px4_state.velocity[0] = msg->twist.linear.x;
            px4_state.velocity[1] = msg->twist.linear.y;
            px4_state.velocity[2] = msg->twist.linear.z;
        }

        void attitude_cb(const sensor_msgs::Imu::ConstPtr& msg)
        {
            Eigen::Quaterniond q_px4 =Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
            //欧拉角表示的姿态  Z-Y-X，即RPY
            Eigen::Vector3d euler_px4 = quaternion_to_euler(q_px4);
             //姿态四元数
            px4_state.attitude_q.w = q_px4.w();
            px4_state.attitude_q.x = q_px4.x();
            px4_state.attitude_q.y = q_px4.y();
            px4_state.attitude_q.z = q_px4.z();
            //欧拉角
            px4_state.attitude[0] = euler_px4[0];
            px4_state.attitude[1] = euler_px4[1];
            px4_state.attitude[2] = euler_px4[2];
            //三个轴的角速度
            px4_state.attitude_rate[0] = msg->angular_velocity.x;
            px4_state.attitude_rate[1] = msg->angular_velocity.y;
            px4_state.attitude_rate[2] = msg->angular_velocity.z;

        }

};


 #endif // 